#ifndef CONTROLLER_ANGPID_H_
#define CONTROLLER_ANGPID_H_

// ------------------------------------------------------
// Includes
// ------------------------------------------------------

#include <math.h>
#include <signal.h>
#include <time.h>
#include <sys/time.h>

#include <cstdlib>
#include <stdio.h>   // Standard input/output definitions
#include <unistd.h>  // UNIX standard function definitions
#include <fcntl.h>   // File control definitions
#include <termios.h> // POSIX terminal control definitions

#include <common/mavlink.h>

// ------------------------------------------------------
// Defines
// ------------------------------------------------------


#define PI 3.1415926


#ifndef HAVE_STRUCT_PID_KIT_T
#define HAVE_STRUCT_PID_KIT_T
typedef struct _PID_kit_t
{
	float Kp;
	float Ki;
	float Kd;
	float u_limit;
	float l_limit;
}PID_kit_t;
#endif


// ------------------------------------------------------
// angle PID controller class
// ------------------------------------------------------

class controller_angPID_c
{

public:
	controller_angPID_c(float _dT);
	controller_angPID_c();
	~controller_angPID_c();
	
	void set(float _Kp, float _Ki, float _Kd, float _upper_bound, float _lower_bound);
	int calc();
	float u_ctrl;
	float ref;
	float feedback;
	float v_feedback;
	float dT;
	float error;
	
private:
	float Kp;
	float Ki;
	float Kd;
	float upper_bound;
	float lower_bound;
	float integrator;
	float ref1;
	float feedback1;
	float error1;
};

#endif